from launch import LaunchDescription
from launch_ros.actions import Node, PushROSNamespace
from launch.actions import IncludeLaunchDescription, GroupAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution, TextSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():

    inputJS = Node(
        package="input",
        executable="joystick",
        name="js_input",
        remappings=[("control_signal", "/robot2/control_signal")],
        output="screen",
    )

    webotsSim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_sim"), "launch", "sim_multi.launch.py"]
            )
        )
    )

    view = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_define"), "launch", "view.launch.py"]
            )
        )
    )

    robotControl1 = GroupAction(
        actions=[
            PushROSNamespace("robot1"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [FindPackageShare("synthesize"), "launch", "robot.launch.py"]
                    )
                ),
                launch_arguments={
                    "servo_mode": TextSubstitution(text="True"),
                }.items(),
            ),
        ]
    )
    robotControl2 = GroupAction(
        actions=[
            PushROSNamespace("robot2"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [FindPackageShare("synthesize"), "launch", "robot.launch.py"]
                    )
                ),
                launch_arguments={
                    "servo_mode": TextSubstitution(text="True"),
                }.items(),
            ),
        ]
    )
    return LaunchDescription([robotControl2, inputJS, webotsSim, view])
    return LaunchDescription([robotControl1, robotControl2, inputJS, webotsSim, view])
